% 求解以下优化问题(到两固定端点距离相等且距离和最小的点的坐标)
% 两个固定端点分别为(0,0)和(10,10)
% x = [x1;x2;x3;y1;y2;y3]α
% 当H变化是，下面的程序也要改！！！
% J=minimize( (x2-x1)^2+(y2-y1)^2 )
% (x2-x1)^2+(y2-y1)^2 = (x3-x2)^2+(y3-y2)^2
% 0<=x1<=0
% 0<=y1<=0
% 0<=x2<=10
% 0<=y2<=12
% 10<=x1<=10
% 10<=y1<=10
% 注意：不要使用x1=0,y1=0,...等等的常数约束！！！，学着将常数约束改成不等式约束0<=x1<=0
% 0<=y1<=0,...
clear
close all

%initial condition
H = 3;
V = 1;
u_max = 0.2;
% w = 1.5;
% delta = 1e-4;

%define objective matrix
Q0 = zeros(7,7);%minimize( (x2-x1)^2+(y2-y1)^2 )
Q0(1,1) = 1;
Q0(1,2) = -1;
Q0(2,1) = -1;
Q0(2,2) = 1;
Q0(4,4) = 1;
Q0(4,5) = -1;
Q0(5,4) = -1;
Q0(5,5) = 1;

Eqcon(1).Q = zeros(7,7); %α^2=1
Eqcon(1).Q(7,7) = 1;
Eqcon(1).c = -1;

Eqcon(2).Q = zeros(7,7);    %(x2-x1)^2+(y2-y1)^2 = (x3-x2)^2+(y3-y2)^2
Eqcon(2).Q(1,1) = 1;
Eqcon(2).Q(1,2) = -1;
Eqcon(2).Q(2,1) = -1;
Eqcon(2).Q(2,3) = 1;
Eqcon(2).Q(3,2) = 1;
Eqcon(2).Q(3,3) = -1;
Eqcon(2).Q(4,4) = 1;
Eqcon(2).Q(4,5) = -1;
Eqcon(2).Q(5,4) = -1;
Eqcon(2).Q(5,6) = 1;
Eqcon(2).Q(6,5) = 1;
Eqcon(2).Q(6,6) = -1;
Eqcon(2).c = 0;

%define inequality constraints
Incon(1).Q = zeros(7,7); %-x1<=0
Incon(1).Q(7,1) = -0.5;
Incon(1).Q(1,7) = -0.5;
Incon(1).c = 0;

Incon(2).Q = zeros(7,7); %x1<=0
Incon(2).Q(7,1) = 0.5;
Incon(2).Q(1,7) = 0.5;
Incon(2).c = 0;

Incon(3).Q = zeros(7,7); %-x2<=0
Incon(3).Q(7,2) = -0.5;
Incon(3).Q(2,7) = -0.5;
Incon(3).c = 0;

Incon(4).Q = zeros(7,7); %x2-10<=0
Incon(4).Q(7,2) = 0.5;
Incon(4).Q(2,7) = 0.5;
Incon(4).c = -10;

Incon(5).Q = zeros(7,7); %-x3+10<=0
Incon(5).Q(7,3) = -0.5;
Incon(5).Q(3,7) = -0.5;
Incon(5).c = 10;

Incon(6).Q = zeros(7,7); %x3-10<=0
Incon(6).Q(7,3) = 0.5;
Incon(6).Q(3,7) = 0.5;
Incon(6).c = -10;

Incon(7).Q = zeros(7,7); %-y1<=0
Incon(7).Q(7,4) = -0.5;
Incon(7).Q(4,7) = -0.5;
Incon(7).c = 0;

Incon(8).Q = zeros(7,7); %y1<=0
Incon(8).Q(7,4) = 0.5;
Incon(8).Q(4,7) = 0.5;
Incon(8).c = 0;

Incon(9).Q = zeros(7,7); %-y2<=0
Incon(9).Q(7,5) = -0.5;
Incon(9).Q(5,7) = -0.5;
Incon(9).c = 0;

Incon(10).Q = zeros(7,7); %y2-12<=0
Incon(10).Q(7,5) = 0.5;
Incon(10).Q(5,7) = 0.5;
Incon(10).c = -12;

Incon(11).Q = zeros(7,7); %-y3+10<=0
Incon(11).Q(7,6) = -0.5;
Incon(11).Q(6,7) = -0.5;
Incon(11).c = 10;

Incon(12).Q = zeros(7,7); %y2-10<=0
Incon(12).Q(7,6) = 0.5;
Incon(12).Q(6,7) = 0.5;
Incon(12).c = -10;

output = irma(Q0,Incon,Eqcon);